import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D


def ortho(left, right, bottom, top, zNear, zFar):
    A = np.identity(4)
    A[0, 0] = 2 / (right - left)
    A[1, 1] = 2 / (top - bottom)
    A[2, 2] = -2 / (zFar - zNear)
    A[0, 3] = -(left + right) / (right - left)
    A[1, 3] = -(top + bottom) / (top - bottom)
    A[2, 3] = -(zFar + zNear) / (zFar - zNear)
    return A


def perspective(fovy, aspect, zNear, zFar):
    A = np.identity(4)
    A[0, 0] = zNear
    A[1, 1] = zNear
    A[2, 2] = zNear + zFar
    A[2, 3] = zNear * zFar
    A[3, 2] = -1
    A[3, 3] = 0

    bottom = -zNear * np.tan(np.radians(fovy) / 2)
    top = -bottom
    left = bottom * aspect
    right = -left

    O = ortho(left, right, bottom, top, zNear, zFar)
    # return O
    return O @ A


def homogeneous_to_cartesian(points):
    """
    将齐次坐标转换为笛卡尔坐标
    """
    return points[0:3] / points[3]

# 测试 [-1,1] 到空间的逆透视
def test_inv():
    # 定义网格范围和分辨率
    x_range = [-1, 1]  # x轴范围
    y_range = [-1, 1]  # y轴范围
    z_range = [-1, 1]  # z轴范围
    step = 0.2  # 网格步长

    # 生成网格点坐标
    x = np.arange(x_range[0], x_range[1] + step, step)
    y = np.arange(y_range[0], y_range[1] + step, step)
    z = np.arange(z_range[0], z_range[1] + step, step)

    # 使用meshgrid生成三维网格[5,7](@ref)
    X, Y, Z = np.meshgrid(x, y, z)

    # 将网格点展平为一维数组并组合成坐标点矩阵[5,7](@ref)
    grid_points = np.vstack([X.ravel(), Y.ravel(), Z.ravel()]).T
    perspective_points = np.zeros((len(grid_points), 3))

    P = perspective(45, 1, 0.1, 100)
    Pi = np.linalg.inv(P)
    for i in range(len(grid_points)):
        # 计算齐次坐标
        point = grid_points[i]
        homogeneous_point = np.array([point[0], point[1], point[2], 1])
        perspective_point = homogeneous_to_cartesian(Pi @ homogeneous_point)
        z = perspective_point[2]
        perspective_point[2] = (-1 / 0.1 - 1 / z) / (- 1 / 0.1 + 1 / 100)
        perspective_points[i] = perspective_point

    print(f"生成了 {len(grid_points)} 个网格点")
    print("前10个网格点的坐标:")
    print(grid_points[:10])
    print(perspective_points[:10])

    # 可视化网格点
    fig = plt.figure(figsize=(10, 8))
    ax = fig.add_subplot(111, projection="3d")

    # 绘制网格点[5,7](@ref)
    scatter = ax.scatter(
        perspective_points[:, 0],
        perspective_points[:, 1],
        perspective_points[:, 2],
        c=grid_points[:, 2], # 根据原始网格的z坐标设置颜色
        cmap="viridis",
        marker="o",
        alpha=0.6,
    )

    plt.tight_layout()
    plt.show()


def test_depth():
    # 定义网格范围和分辨率
    z_range = [-0.1, -100]  # z轴范围
    step = -0.2  # 网格步长
    z = np.arange(z_range[0], z_range[1] + step, step)

    grid_points = np.zeros((len(z), 3))
    for i in range(len(z)):
        grid_points[i] = [0, 0, z[i]]

    P = perspective(45, 1, 0.1, 100)
    perspective_points = np.zeros((len(grid_points), 3))
    for i in range(len(grid_points)):
        point = grid_points[i]

        # 深度映射
        z = point[2]
        # z = -(point[2] + 0.1) / (100 - 0.1)
        # z = -1.0 / (z * (-1 / 0.1 + 1 / 100) + 1 / 0.1)

        homogeneous_point = np.array([point[0], point[1], z, 1])
        perspective_point = homogeneous_to_cartesian(P @ homogeneous_point)
        # perspective_point = homogeneous_to_cartesian(homogeneous_point)
        z = perspective_point[2]
        z = 1 / (z * (-1 / 0.1 + 1 / 100) + 1 / 0.1)
        z = (z - 0.1) / (100 - 0.1)
        perspective_point[2] = z
        perspective_points[i] = perspective_point

    # 可视化网格点
    fig = plt.figure(figsize=(10, 8))
    ax = fig.add_subplot(111, projection="3d")

    # 绘制网格点[5,7](@ref)
    scatter = ax.scatter(
        perspective_points[:, 0],
        perspective_points[:, 1],
        perspective_points[:, 2],
        c=grid_points[:, 2],
        cmap="viridis",
        marker="o",
        alpha=0.6,
    )

    plt.tight_layout()
    plt.show()


test_depth()